#include "CHaptikDevice.h"

#pragma warning(disable: 4244)
#pragma warning(disable: 4996)

const double DEVICE_WORKSPACE_HALF_SIZE = 50.0;

cHaptikDevice::cHaptikDevice(){

	m_hapticDevice = NULL;
	m_systemAvailable = false;
    m_systemReady = false;
	m_haptikStarted = false;
	m_hapConnected = 0;
	m_returnType = 0;

	//Allocate the pointer to the library
	m_haptik = new Haptik();

}

int cHaptikDevice::startHaptik(){

    //Number of connected devices
	m_numDevices = 0;

	//Return value
	m_returnType = RET_VAL_OK;

	//Get informations about the library
	m_haptik->library->GetLibraryInfo(m_libraryInfo);

	m_infoLibrary.m_libCompany = m_libraryInfo.company;
	m_infoLibrary.m_libName    = m_libraryInfo.name;
	m_infoLibrary.m_libVersion = m_libraryInfo.version;

	//Refresh the list of the connected devices
	UINT32 uRes = m_haptik->RefreshDeviceList();
	if FAILED(uRes)
	{  
	   m_systemReady = false;
       m_systemAvailable = false;
	   m_returnType = RET_VAL_NOREFRESH;
	   return m_returnType;
	}

	//No connected devices
    if (m_haptik->numberOfDevices == 0)
    {
	   m_systemReady = false;
       m_systemAvailable = false;
	   m_returnType = RET_VAL_NODEVICES;
	   return m_returnType;
    }

	m_numDevices = m_haptik->numberOfDevices;
	m_haptikStarted = true;

    return m_returnType;

}

int cHaptikDevice::getHaptikDevice(UINT32 a_inDevice){

  m_returnType = RET_VAL_OK;

  if(!m_haptikStarted){
	  m_returnType = RET_VAL_LISTNOTREFRESHED;
      return m_returnType;
  }

   if(a_inDevice >= 0 && a_inDevice < m_numDevices)
   {
   //Get the interface for the device
   if(connectDevice(a_inDevice,true) == 1)
      m_systemAvailable = true;  

   else{
	  //Device not available
      m_systemAvailable = false;
	  m_returnType = RET_VAL_DEVICENOTAVAILABLE;
	  return m_returnType;
       }
   }

   else
   {
	  //Incorrect choose, assign DEFAULT-DEVICE
	  if(connectDevice(HAPTIK_DEFAULT_DEVICE,false) == 1){
	  m_returnType = RET_VAL_DEFAULTDEVICE;
      m_systemAvailable = true;
	  }

	  else{
	  //DEFAULT DEVICE not available
      m_systemAvailable = false;
	  m_returnType = RET_VAL_DEFAULTNOTAVAILABLE;
	  return m_returnType;
      }
   }

   return m_returnType;

}

int cHaptikDevice::connectDevice(UINT32 a_numberDevice, bool a_correctChoice){

	//Request interface for the device
	if(a_correctChoice)
	m_hapticDevice = (IHaptikDeviceInterface)m_haptik->GetDeviceInterface(m_haptik->device[a_numberDevice].id);
	else
	//Assign DEFAULT DEVICE (first available device)
	m_hapticDevice = (IHaptikDeviceInterface)m_haptik->GetDeviceInterface(a_numberDevice);
	
	//Request failed, device not connected
	if (m_hapticDevice == NULL)
   {
      m_hapConnected = 0;
	  return m_hapConnected;
   }

   else

   {
	   //Device connected
	   m_hapConnected = 1;

	   if(!a_correctChoice)
	   m_defaultDevice = 1;

	   //Read informations and memorize them in m_deviceInfo
	   m_hapticDevice->GetInfo(m_deviceInfo);

	   m_featuresDevice.m_deviceId = m_deviceInfo.id;
	   m_featuresDevice.m_deviceName = m_deviceInfo.name;
	   m_featuresDevice.m_deviceManufacturer = m_deviceInfo.manufacturer;
	   m_featuresDevice.m_deviceModel = m_deviceInfo.model;
	   m_featuresDevice.m_rate = m_deviceInfo.currentRate;
	   m_featuresDevice.m_numOfButtons = m_deviceInfo.numberOfButtons;
	   m_featuresDevice.m_hwCapabilities = m_deviceInfo.capabilities;

	   m_wokspaceDevice.m_maxWorkspaceX = m_deviceInfo.maxX;
	   m_wokspaceDevice.m_minWorkspaceX = m_deviceInfo.minX;
	   m_wokspaceDevice.m_maxWorkspaceY = m_deviceInfo.maxY;
	   m_wokspaceDevice.m_minWorkspaceY = m_deviceInfo.minY;
	   m_wokspaceDevice.m_maxWorkspaceZ = m_deviceInfo.maxZ;
	   m_wokspaceDevice.m_minWorkspaceZ = m_deviceInfo.minZ;
	   m_wokspaceDevice.m_maxWorkspace = m_deviceInfo.maxWorkspace;

	   m_forceAndTorqueDevice.m_maxForce = m_deviceInfo.maxForceIntensity;
	   m_forceAndTorqueDevice.m_maxTorque = m_deviceInfo.maxTorqueIntensity;

	   //Scale for position and force conversion in the workspace
       m_scalePositionX = (double)m_wokspaceDevice.m_maxWorkspaceX;
       m_scalePositionX = m_scalePositionX / 2.0;
	   m_scalePositionY = (double)m_wokspaceDevice.m_maxWorkspaceY;
       m_scalePositionY = m_scalePositionY / 2.0;
	   m_scalePositionZ = (double)m_wokspaceDevice.m_maxWorkspaceZ;
       m_scalePositionZ = m_scalePositionZ / 2.0;

	   m_scaleForce = (double)m_forceAndTorqueDevice.m_maxForce;
	   m_scaleTorque = (double)m_forceAndTorqueDevice.m_maxTorque;

	   return m_hapConnected;
	   }
}

cHaptikDevice::~cHaptikDevice(){

	if(m_hapticDevice){
	if(m_systemAvailable){
	close();
	release();
	}
	}

	if(m_haptik)
	delete m_haptik;
}

int cHaptikDevice::initialize(const bool a_resetEncoders){

	m_returnType = RET_VAL_OK;

	if (!m_systemAvailable){
		m_returnType = RET_VAL_SYSNOTAVAILABLE;
		return m_returnType;
	}
   
   //Init device
   UINT32 uRes = m_hapticDevice->Init();

   if FAILED(uRes)
   {
	   if(uRes == HAPTIK_SDK_ERROR){
		 m_returnType = RET_VAL_SDK;
	   }
	   else if(uRes == HAPTIK_MEMORY_ERROR){
		 m_returnType = RET_VAL_MEMORY;
	   }

	   else m_returnType = RET_VAL_CANNOTINIT;

      RELEASE_HAPTIK_INTERFACE(m_hapticDevice);
      return m_returnType;
   }

   //Control calibration status
   UINT32 uCal = m_hapticDevice->GetCalibrationStatus();

   if(uCal == HAPTIK_NOTCALIBRATED || a_resetEncoders == true){

	 //Device not calibrated, recalibrate
	 UINT32 uRes_c = m_hapticDevice->Recalibrate();
	 if FAILED(uRes_c)
	 {
		 RELEASE_HAPTIK_INTERFACE(m_hapticDevice);
		 m_returnType = RET_VAL_NORECALIBRATE;
         return m_returnType;
	 }

	   m_systemReady = true;
   }

   else{
        m_systemReady = true;
       }

	return m_returnType;
}

int cHaptikDevice::open(){

	m_returnType = RET_VAL_OK;

    //Start device
	if(!m_systemAvailable){
		m_returnType = RET_VAL_SYSNOTAVAILABLE;
		return m_returnType;
	}

   UINT32 uRes = m_hapticDevice->Start();

   if(uRes == HAPTIK_ALREADY)
	   m_returnType = RET_VAL_ALREADY;

   if FAILED(uRes)
   {
      RELEASE_HAPTIK_INTERFACE(m_hapticDevice);
	  m_returnType = RET_VAL_CANNOTSTART;
      return m_returnType;
   }

   return m_returnType;

}

int cHaptikDevice::close(){

   m_returnType = RET_VAL_OK;

   //Stop device
   if(!m_systemAvailable){
		m_returnType = RET_VAL_SYSNOTAVAILABLE;
		return m_returnType;
   }

   m_hapticDevice->Stop();
   
   m_systemReady = false;

   return m_returnType;

}

//Release the obtained interface
void cHaptikDevice::release(){

	RELEASE_HAPTIK_INTERFACE(m_hapticDevice);
	m_systemAvailable = false;
	m_systemReady = false;
	m_haptikStarted = false;
	m_hapConnected = 0;
	m_returnType = 0;

}

//Commands to/from virtual environment
int cHaptikDevice::command(int a_command, void* a_data)
{
    int result = CHAI_MSG_OK;
    double x = 0.0, y = 0.0, z = 0.0;
	//Scaled position variables
	double xs = 0.0, ys = 0.0, zs = 0.0;

    if (m_systemReady)
    {
        switch (a_command)
		{
            //Read non-scaled position of the device
            case CHAI_CMD_GET_POS_3D:
            {
				m_hapticDevice->Read(m_data);
                x = (double)m_data.position.x;
                y = (double)m_data.position.y;
                z = (double)m_data.position.z;

                cVector3d* position = (cVector3d *) a_data;
                position->set(x, y, z);
            }
            break;

			//Read position and scale it
            case CHAI_CMD_GET_POS_NORM_3D:
            {
				m_hapticDevice->Read(m_data);
                x = (double)m_data.position.x;
                y = (double)m_data.position.y;
                z = (double)m_data.position.z;

				//Scale factor for every axis
				//Usually are the same
				xs = x / m_scalePositionX;
				ys = y / m_scalePositionY;
				zs = z / m_scalePositionZ;

                cVector3d* position = (cVector3d *) a_data;
                position->set(xs, ys, zs);
              //position->div(DEVICE_WORKSPACE_HALF_SIZE);
            }
            break;

            //Read rotation angles
			//Set (0,0,0)
            case CHAI_CMD_GET_ROT_ANGLES:
            {
                cVector3d* angles = (cVector3d *) a_data;
                angles->set(0, 0, 0);
            }
            break;

            //Read rotation matrix
            case CHAI_CMD_GET_ROT_MATRIX:
            {
				m_hapticDevice->Read(m_data);
                cMatrix3d* matrix = (cMatrix3d *) a_data;
				//Original matrix [4]x[4]
				m_wokspaceDevice.m_originalMatrix = m_data.matrix;
				//Exclude fourth row and fourth column
				//Set the data
				matrix->set(m_data.matrix.e00,m_data.matrix.e01,m_data.matrix.e02,
					m_data.matrix.e10,m_data.matrix.e11,m_data.matrix.e12,
					m_data.matrix.e20,m_data.matrix.e21,m_data.matrix.e22);
            }

            break;

            //Send scaled forces to the device according to max intensity
            case CHAI_CMD_SET_FORCE_3D:
            {
                cVector3d* force = (cVector3d *) a_data;

				m_data.forceFeedback.x = (force->x/m_scaleForce);
				m_data.forceFeedback.y = (force->y/m_scaleForce);
				m_data.forceFeedback.z = (force->z/m_scaleForce);
				m_hapticDevice->Write(m_data);

            }
            break;

            //Send scaled torque
            case CHAI_CMD_SET_TORQUE_3D:
            {
				cVector3d* torque = (cVector3d *) a_data;
				m_data.torqueFeedback.x = (torque->x/m_scaleTorque);
				m_data.torqueFeedback.y = (torque->y/m_scaleTorque);
				m_data.torqueFeedback.z = (torque->z/m_scaleTorque);
				m_hapticDevice->Write(m_data);
            }
            break;

			//Read velocity of the device
			case CHAI_CMD_GET_VEL_3D:
           {
                cVector3d* velocity = (cVector3d *) a_data;
                
				m_hapticDevice->Read(m_data);
				velocity->x =  m_data.velocity.x;
				velocity->y =  m_data.velocity.y;
				velocity->z =  m_data.velocity.z;
           }
		   break;

            //Read button mask
            case CHAI_CMD_GET_SWITCH_MASK:

				{
			    int* result = (int *) a_data;
				m_hapticDevice->Read(m_data);
				*result = (int)m_data.buttonMask;
				}

				break;

            case CHAI_CMD_GET_SWITCH_0:
            {
                int* result = (int *) a_data;
				m_hapticDevice->Read(m_data);
				*result = m_data.Button(0) ? 1 : 0;
            }
            break;

			case CHAI_CMD_GET_SWITCH_1:
            {
                int* result = (int *) a_data;
				m_hapticDevice->Read(m_data);
				*result = m_data.Button(1) ? 1 : 0;
            }
            break;

			case CHAI_CMD_GET_SWITCH_2:
            {
                int* result = (int *) a_data;
				m_hapticDevice->Read(m_data);
				*result = m_data.Button(2) ? 1 : 0;
            }
            break;

			//Request the scale factor
			case CHAI_CMD_GET_NORMALIZED_SCALE_FACTOR:
            {
                double* scale = (double*)a_data;
                *scale = (m_scalePositionX + m_scalePositionY + m_scalePositionZ) / 3.0;
                result = CHAI_MSG_OK;
            }
            break;

            default:
                result = CHAI_MSG_NOT_IMPLEMENTED;
        }
    }
    else
    {
        result = CHAI_MSG_SYSTEM_NOT_READY;
    }

    return (result);
}

int cHaptikDevice::modifyDevice(int a_choose, void* a_data)
{
	int res = 0;

	if(m_hapticDevice == NULL){
		res = RET_VAL_SYSNOTAVAILABLE;
		return res;
	}

	switch(a_choose){

	case 0: { Float32* rate = (Float32*) a_data;
			  *rate = m_hapticDevice->GetRate();
			}
	break;

	case 1: { Float32* rate = (Float32*) a_data;
		      m_hapticDevice->SetRate(*rate);
			}
	break;

	case 2: { UINT32* cal_status = (UINT32*) a_data;
		      *cal_status = m_hapticDevice->GetCalibrationStatus();
			}
	break;

	case 3: { UINT32* last_error = (UINT32*) a_data;
		      *last_error = m_hapticDevice->GetLastError();
			}
	break;

	case 4: { UINT32* recalibrate = (UINT32*) a_data;
		      *recalibrate = m_hapticDevice->Recalibrate();
			  if FAILED(*recalibrate)
	 {
		 RELEASE_HAPTIK_INTERFACE(m_hapticDevice);
         res = RET_VAL_NORECALIBRATE;
	 }
			
			}
	break;

	default: res = RET_VAL_OK;

	}

	return res;
}

const Char8* cHaptikDevice::resultToString(int a_result){

	const Char8* res;

	switch(a_result){

	case RET_VAL_ALREADY:
    res = "\nThe device has been already activated\n";
	break;

	case RET_VAL_DEFAULTDEVICE:
	res = "\nIncorrect choice - default device assigned\n";
	break;

	case RET_VAL_NOREFRESH:
    res = "\nCannot refresh the list of devices\n";
	break;

	case RET_VAL_NODEVICES:
    res = "\nNo device connected\n";
	break;

	case RET_VAL_DEVICENOTAVAILABLE:
    res = "\nDevice not available\n";
	break;

	case RET_VAL_DEFAULTNOTAVAILABLE:
    res = "\nIncorrect choice - Default device not available\n";
	break;

	case RET_VAL_LISTNOTREFRESHED:
    res = "\nThe list of devices has not been refreshed yet!\n";
	break;

	case RET_VAL_SYSNOTAVAILABLE:
	res = "\nThe system is not available\n";
	break;

	case RET_VAL_CANNOTINIT:
	res = "\nCannot initialize the device\n";
	break;

	case RET_VAL_CANNOTSTART:
	res = "\nCannot start the device\n";
	break;

	case RET_VAL_NORECALIBRATE:
    res = "\nCannot recalibrate the device\n";
	break;

	case RET_VAL_SDK:
	res = m_haptik->ResultToString(HAPTIK_SDK_ERROR);
	break;
		
	case RET_VAL_MEMORY:
	res = m_haptik->ResultToString(HAPTIK_MEMORY_ERROR);
	break;

	default:
	res = "\nError not identified\n";
	
	}

	return res;
}